#include "BSpline.h"
#include <Eigen/Core>
#include <iostream>
#include <fstream>
#include <random>
#include <map>

#ifndef M_PI
#define M_PI       3.14159265358979323846
#endif


void saveResultAsOBJ(const std::string& filename, const std::vector<Eigen::Vector3d>& polylines)
{
	std::ofstream file;
	file.open(filename.c_str());

	std::size_t lineID = 0;
	std::size_t pointID = 1;
	std::map<std::size_t, std::size_t> lines2points;
	for (size_t i = 0; i < polylines.size() - 1; ++i, ++lineID, pointID += 2)
	{
		Eigen::Vector3d P1 = polylines[i];
		Eigen::Vector3d P2 = polylines[i + 1];

		file << "v " << P1.x() << " " << P1.y() << " " << P1.z() << std::endl;
		file << "v " << P2.x() << " " << P2.y() << " " << P2.z() << std::endl;

		lines2points[lineID] = pointID;
	}

	std::map<size_t, size_t>::const_iterator it = lines2points.begin();
	for (; it != lines2points.end(); ++it)
	{
		file << "l " << it->second << " " << it->second + 1 << std::endl;
	}

	file.close();
}


int main()
{
	std::random_device rd;
	std::default_random_engine generator_(rd());
	std::normal_distribution<double> noise(0.0, 1.0);
	std::vector<Eigen::Vector3d> sample_points, fitting_line;
	
	double k = M_PI / 10.0;
	double ellipse_x = 15.0;
	for (int t = 0; t < 100; t++)
	{
		Eigen::Vector3d noise_gyro(noise(generator_), 0.0, 0.0);
		Eigen::Vector3d position = Eigen::Vector3d(ellipse_x * cos(k * t), t, 0.0)/* + noise_gyro*/;
		sample_points.push_back(position);
	}

	//BSpline bs = BSpline::CubicApproximate(sample_points, 1.0, 1.0);
	BSpline bs = BSpline::Interpolate3Degree(sample_points);
	for (int i = 3; i < bs.m_vecKnots.size() - 4; i++)
	{
		fitting_line.push_back(bs.Evaluate(bs.m_vecKnots[i]));
	}

	saveResultAsOBJ("ctrl.obj", bs.m_vecCVs);
	saveResultAsOBJ("sample.obj", sample_points);
	saveResultAsOBJ("fitting.obj", fitting_line);
	return -1;
}